#include "main.h"

I2C i2c(PB_14, PB_13); 
MPU6050 mpu(&i2c);
IMUData imudata = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};

float gyro_bias[3] = {0};  
void calibrateGyroBias() {
    float g[3];
    const int samples = 30;  
    for (int i = 0; i < samples; ++i) {
        mpu.readGyroData(g);
        gyro_bias[0] += g[0];
        gyro_bias[1] += g[1];
        gyro_bias[2] += g[2];
        ThisThread::sleep_for(3ms); 
    }
    gyro_bias[0] /= samples;
    gyro_bias[1] /= samples;
    gyro_bias[2] /= samples;
}


void gyro_init() {
    mpu.init(); 
    calibrateGyroBias();
}

void data_Read(IMUData &data) {
    if (gyro_slow) return;
    gyro_slow = 1;

    float accel[3], gyro[3], temp;

    mpu.readAccelData(accel);
    mpu.readGyroData(gyro);
    temp = mpu.readTempData();

    gyro[0] -= gyro_bias[0];
    gyro[1] -= gyro_bias[1];
    gyro[2] -= gyro_bias[2];

    data.ax = accel[0];
    data.ay = accel[1];
    data.az = accel[2];

    data.gx = gyro[0];
    data.gy = gyro[1];
    data.gz = gyro[2];

    data.temp = temp;
    //printf("GYRO Y: %.2f \n", data.gy);
}

